#include <iostream>
#include <fstream>
#include <string>
#include <cstring>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <poll.h>
#include <linux/videodev2.h>
#include <errno.h>
#include <signal.h>
#include <chrono>
#include <iomanip>
#include <sstream>
#include <getopt.h>

// ========== 多线程优化版本 ==========
#include <queue>
#include <mutex>
#include <condition_variable>
#include <atomic>
#include <thread>

class V4L2Camera {
private:
    int video_fd;
    std::string video_device;
    int width, height;
    bool save_files;  // 是否保存文件的标志
    
    // 内存映射相关
    struct Buffer {
        void* start;
        size_t length;
        int index;
    };
    Buffer* buffers;
    unsigned int n_buffers;
    
    // 计时相关成员变量
    std::chrono::steady_clock::time_point first_frame_time;
    std::chrono::steady_clock::time_point last_frame_time;
    bool first_frame_captured;
    
public:
    V4L2Camera(const std::string& video_dev = "/dev/video11", bool save = false) 
        : video_device(video_dev), width(480), height(480), save_files(save),
          buffers(nullptr), n_buffers(0), first_frame_captured(false) {}
    
    ~V4L2Camera() {
        cleanup();
    }
    
    // 获取第一帧时间
    std::chrono::steady_clock::time_point getFirstFrameTime() const {
        return first_frame_time;
    }
    
    // 获取最后一帧时间
    std::chrono::steady_clock::time_point getLastFrameTime() const {
        return last_frame_time;
    }
    
    // 检查是否已捕获第一帧
    bool hasFirstFrame() const {
        return first_frame_captured;
    }
    
    bool openVideoDevice() {
        video_fd = open(video_device.c_str(), O_RDWR);
        if (video_fd == -1) {
            std::cerr << "无法打开视频设备 " << video_device << ": " << strerror(errno) << std::endl;
            return false;
        }
        std::cout << "成功打开视频设备: " << video_device << std::endl;
        return true;
    }
    
    bool setVideoFormat(int width, int height, __u32 pixelformat) {
        this->width = width;
        this->height = height;
        
        // 专注多平面格式，sc035gs MIPI摄像头
        v4l2_format fmt;
        memset(&fmt, 0, sizeof(fmt));
        fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        fmt.fmt.pix_mp.width = width;
        fmt.fmt.pix_mp.height = height;
        fmt.fmt.pix_mp.pixelformat = pixelformat;
        fmt.fmt.pix_mp.field = V4L2_FIELD_NONE;
        fmt.fmt.pix_mp.num_planes = 2; // NV12有2个平面
        
        if (ioctl(video_fd, VIDIOC_S_FMT, &fmt) < 0) {
            std::cerr << "设置多平面格式失败: " << strerror(errno) << std::endl;
            return false;
        }
        
        std::cout << "设置多平面视频格式成功: " << this->width << "x" << this->height 
                  << ", 像素格式: " << (char*)&pixelformat << std::endl;
        return true;
    }
    
    bool initMemoryMapping() {
        const int buffer_count = 4; // 增加到4个缓冲区，支持180fps
        
        v4l2_requestbuffers req;
        memset(&req, 0, sizeof(req));
        req.count = buffer_count;
        req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; // 专注多平面
        req.memory = V4L2_MEMORY_MMAP;
        
        if (ioctl(video_fd, VIDIOC_REQBUFS, &req) < 0) {
            std::cerr << "请求多平面缓冲区失败: " << strerror(errno) << std::endl;
            return false;
        }
        
        if (req.count < 1) {
            std::cerr << "无法分配缓冲区" << std::endl;
            return false;
        }
        
        std::cout << "实际分配的缓冲区数量: " << req.count << std::endl;
        
        n_buffers = req.count; // 使用实际分配的缓冲区数量
        buffers = new Buffer[n_buffers];
        
        for (unsigned int i = 0; i < n_buffers; i++) {
            // 专注多平面缓冲区，sc035gs MIPI摄像头
            v4l2_plane planes[VIDEO_MAX_PLANES];
            v4l2_buffer buf;
            memset(&buf, 0, sizeof(buf));
            buf.index = i;
            buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
            buf.memory = V4L2_MEMORY_MMAP;
            buf.m.planes = planes;
            buf.length = 1;
            
            if (ioctl(video_fd, VIDIOC_QUERYBUF, &buf) < 0) {
                std::cerr << "查询多平面缓冲区 " << i << " 失败: " << strerror(errno) << std::endl;
                return false;
            }
            
            // 映射第一个平面（Y分量），参考180fps.txt的mmap策略
            buffers[i].start = mmap(NULL, buf.m.planes[0].length, PROT_READ | PROT_WRITE, 
                                   MAP_SHARED, video_fd, buf.m.planes[0].m.mem_offset);
            if (buffers[i].start == MAP_FAILED) {
                std::cerr << "映射缓冲区 " << i << " 失败: " << strerror(errno) << std::endl;
                return false;
            }
            
            buffers[i].length = buf.m.planes[0].length;
            buffers[i].index = i;
            
            std::cout << "多平面缓冲区 " << i << " 映射成功，长度: " << buf.m.planes[0].length << " 字节" << std::endl;
        }
        
        std::cout << "内存映射初始化成功，缓冲区数量: " << n_buffers << std::endl;
        return true;
    }
    
    std::string generateTimestamp() {
        auto now = std::chrono::system_clock::now();
        auto time_t = std::chrono::system_clock::to_time_t(now);
        auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()) % 1000;
        
        std::stringstream ss;
        ss << "trig-" 
           << std::put_time(std::localtime(&time_t), "%H%M%S")
           << "-" << std::setfill('0') << std::setw(3) << ms.count();
        
        return ss.str();
    }
    
    bool startStreaming() {
        // 开始流，专注多平面
        __u32 type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        
        if (ioctl(video_fd, VIDIOC_STREAMON, &type) < 0) {
            std::cerr << "开始多平面流失败: " << strerror(errno) << std::endl;
            return false;
        }
        
        std::cout << "多平面流启动成功，开始循环捕获..." << std::endl;
        return true;
    }
    
    bool stopStreaming() {
        // 停止流，专注多平面
        __u32 type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        
        if (ioctl(video_fd, VIDIOC_STREAMOFF, &type) < 0) {
            std::cerr << "停止多平面流失败: " << strerror(errno) << std::endl;
            return false;
        } else {
            std::cout << "多平面流已停止" << std::endl;
        }
        return true;
    }
    
    bool captureFrame() {
        // 专注多平面缓冲区处理，sc035gs MIPI 180fps摄像头
        static int current_buffer = 0;
        v4l2_plane planes[VIDEO_MAX_PLANES];
        memset(planes, 0, sizeof(planes));
        
        v4l2_buffer buf;
        memset(&buf, 0, sizeof(buf));
        buf.index = current_buffer;  // 使用循环缓冲区
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.m.planes = planes;
        buf.length = 1;
        
        // 设置平面信息
        planes[0].m.mem_offset = 0;  // 第一个平面的内存偏移
        planes[0].length = buffers[current_buffer].length;  // 当前缓冲区的长度
        
        if (ioctl(video_fd, VIDIOC_QBUF, &buf) < 0) {
            std::cerr << "入队多平面缓冲区失败: " << strerror(errno) << std::endl;
            return false;
        }
        
        std::cout << "多平面缓冲区 " << current_buffer << " 入队成功" << std::endl;
        
        // 使用poll等待，参考180fps.txt，更高效
        struct pollfd pfd;
        pfd.fd = video_fd;
        pfd.events = POLLIN;
        pfd.revents = 0;
        
        int r = poll(&pfd, 1, 100); // 100ms超时，平衡响应和稳定性
        if (r == -1) {
            std::cerr << "poll失败: " << strerror(errno) << std::endl;
            // 如果poll失败，需要出队缓冲区
            memset(&buf, 0, sizeof(buf));
            buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
            buf.memory = V4L2_MEMORY_MMAP;
            buf.m.planes = planes;
            buf.length = 1;
            ioctl(video_fd, VIDIOC_DQBUF, &buf); // 忽略错误
            return false;
        } else if (r == 0) {
            std::cout << "等待帧数据超时，需要出队缓冲区" << std::endl;
            // 超时时，需要出队缓冲区
            memset(&buf, 0, sizeof(buf));
            buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
            buf.memory = V4L2_MEMORY_MMAP;
            buf.m.planes = planes;
            buf.length = 1;
            if (ioctl(video_fd, VIDIOC_DQBUF, &buf) < 0) {
                std::cerr << "超时后出队缓冲区失败: " << strerror(errno) << std::endl;
            } else {
                std::cout << "超时后缓冲区出队成功" << std::endl;
            }
            return false;
        }
        
        std::cout << "检测到帧数据就绪" << std::endl;
        
        // 出队缓冲区
        memset(&buf, 0, sizeof(buf));
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.m.planes = planes;
        buf.length = 1;
        
        if (ioctl(video_fd, VIDIOC_DQBUF, &buf) < 0) {
            std::cerr << "出队多平面缓冲区失败: " << strerror(errno) << std::endl;
            return false;
        }
        
        std::cout << "多平面缓冲区出队成功，索引: " << buf.index << std::endl;
        
        // 记录时间
        auto current_time = std::chrono::steady_clock::now();
        if (!first_frame_captured) {
            first_frame_time = current_time;
            first_frame_captured = true;
            std::cout << "第一帧捕获成功，开始计时..." << std::endl;
        } else {
            // 计算帧间隔
            auto frame_interval = std::chrono::duration_cast<std::chrono::microseconds>(
                current_time - last_frame_time);
            std::cout << "帧间隔: " << frame_interval.count() << " μs (" 
                      << frame_interval.count() / 1000.0 << " ms)" << std::endl;
        }
        last_frame_time = current_time;
        
        std::cout << "捕获到帧数据，大小: " << planes[0].bytesused << " 字节" << std::endl;
        
        // 根据save_files标志决定是否保存文件
        if (save_files) {
            // 生成时间戳文件名
            std::string timestamp = generateTimestamp();
            
            // 直接保存NV12格式的.raw文件
            std::string raw_filename = timestamp + ".raw";
            FILE* outfile = fopen(raw_filename.c_str(), "wb");
            if (!outfile) {
                std::cerr << "无法创建RAW文件: " << raw_filename << std::endl;
                return false;
            }
            
            // 写入NV12数据
            size_t written = fwrite(buffers[buf.index].start, 1, planes[0].bytesused, outfile);
            fclose(outfile);
            
            if (written != planes[0].bytesused) {
                std::cerr << "写入RAW文件失败，预期写入: " << planes[0].bytesused << " 字节，实际写入: " << written << " 字节" << std::endl;
                return false;
            }
            
            std::cout << "NV12格式图像已保存到: " << raw_filename << " (" << written << " 字节)" << std::endl;
        } else {
            std::cout << "帧数据已捕获（未保存文件）" << std::endl;
        }
        
        // 切换到下一个缓冲区
        current_buffer = (current_buffer + 1) % n_buffers;
        
        return true;
    }
    
    void cleanup() {
        if (buffers) {
            for (unsigned int i = 0; i < n_buffers; ++i) {
                if (buffers[i].start) {
                    munmap(buffers[i].start, buffers[i].length);
                }
            }
            delete[] buffers;
            buffers = nullptr;
        }
        
        if (video_fd != -1) {
            close(video_fd);
            video_fd = -1;
        }
    }
};

// 全局变量，用于信号处理
V4L2Camera* g_camera = nullptr;
volatile bool g_running = true;
volatile int g_frame_count = 0;  // 添加全局帧计数器

// 信号处理函数
void signalHandler(int signum) {
    std::cout << "\n接收到信号 " << signum << "，正在关闭摄像头..." << std::endl;
    g_running = false;
    
    // 输出完整的统计信息
    if (g_camera && g_camera->hasFirstFrame()) {
        auto end_time = std::chrono::steady_clock::now();
        auto total_duration = std::chrono::duration_cast<std::chrono::milliseconds>(
            end_time - g_camera->getFirstFrameTime());
        
        std::cout << "\n========== 中断统计信息 ==========" << std::endl;
        std::cout << "程序被中断，总运行时长: " << total_duration.count() << " 毫秒" << std::endl;
        std::cout << "抓取成功的总次数: " << g_frame_count << " 次" << std::endl;
        
        if (g_frame_count > 1) {
            auto avg_interval = total_duration.count() / (g_frame_count - 1);
            std::cout << "抓取的平均间隔: " << avg_interval << " 毫秒" << std::endl;
        } else {
            std::cout << "抓取的平均间隔: 无法计算（次数不足）" << std::endl;
        }
        std::cout << "================================" << std::endl;
    }
    
    if (g_camera) {
        g_camera->stopStreaming();
        g_camera->cleanup();
    }
    exit(0);
}

void printUsage(const char* program_name) {
    std::cout << "用法: " << program_name << " [选项]" << std::endl;
    std::cout << "选项:" << std::endl;
    std::cout << "  -s              保存捕获的图像文件到磁盘" << std::endl;
    std::cout << "  -h, --help      显示此帮助信息" << std::endl;
    std::cout << std::endl;
    std::cout << "示例:" << std::endl;
    std::cout << "  " << program_name << "          # 仅捕获帧，不保存文件" << std::endl;
    std::cout << "  " << program_name << " -s       # 捕获帧并保存为RAW文件" << std::endl;
}

int main(int argc, char* argv[]) {
    bool save_files = false;
    int opt;
    
    // 系统级优化：尝试设置实时优先级
    struct sched_param param;
    param.sched_priority = 80; // 高优先级
    if (sched_setscheduler(0, SCHED_FIFO, &param) == 0) {
        std::cout << "已设置实时调度优先级: " << param.sched_priority << std::endl;
    } else {
        std::cout << "无法设置实时优先级，使用默认调度: " << strerror(errno) << std::endl;
    }
    
    // 解析命令行参数
    while ((opt = getopt(argc, argv, "sh")) != -1) {
        switch (opt) {
            case 's':
                save_files = true;
                break;
            case 'h':
                printUsage(argv[0]);
                return 0;
            default:
                std::cerr << "未知选项，使用 -h 查看帮助" << std::endl;
                return 1;
        }
    }
    
    std::cout << "V4L2 摄像头循环捕获程序 (sc035gs MIPI 180fps 优化版)" << std::endl;
    std::cout << "=================================================" << std::endl;
    std::cout << "文件保存模式: " << (save_files ? "启用" : "禁用") << std::endl;
    std::cout << "按 Ctrl+C 停止程序" << std::endl;
    
    // 设置信号处理
    signal(SIGINT, signalHandler);
    signal(SIGTERM, signalHandler);
    
    V4L2Camera camera("/dev/video11", save_files);
    g_camera = &camera;
    
    // 1. 打开视频设备
    if (!camera.openVideoDevice()) {
        return -1;
    }
    
    // 2. 设置视频格式 (480x480, NV12)
    if (!camera.setVideoFormat(480, 480, V4L2_PIX_FMT_NV12)) {
        return -1;
    }
    
    // 3. 初始化内存映射
    if (!camera.initMemoryMapping()) {
        return -1;
    }
    
    // 4. 开始流
    if (!camera.startStreaming()) {
        std::cerr << "开始流失败" << std::endl;
        return -1;
    }
    
    // 5. 循环捕获帧数据
    int frame_count = 0;
    int consecutive_failures = 0;
    const int max_failures = 5; // 减少最大连续失败次数
    
    while (g_running) {
        if (!camera.captureFrame()) {
            consecutive_failures++;
            if (consecutive_failures > max_failures) {
                std::cerr << "连续失败次数过多，退出程序" << std::endl;
                break;
            }
            // 短暂等待后重试
            usleep(10000); // 10ms
            continue;
        }
        
        consecutive_failures = 0; // 重置失败计数
        frame_count++;
        g_frame_count = frame_count;
        
        // 平衡优化：找到性能和稳定性的最佳平衡点
        usleep(2000); // 2ms延时，平衡性能和稳定性
        
        // 每10帧输出一次状态和性能统计
        if (frame_count % 10 == 0) {
            auto current_time = std::chrono::steady_clock::now();
            if (camera.hasFirstFrame()) {
                auto total_duration = std::chrono::duration_cast<std::chrono::milliseconds>(
                    current_time - camera.getFirstFrameTime());
                double avg_interval = total_duration.count() / (double)frame_count;
                std::cout << "已捕获 " << frame_count << " 帧, 平均间隔: " 
                          << std::fixed << std::setprecision(2) << avg_interval << " ms" << std::endl;
            } else {
                std::cout << "已捕获 " << frame_count << " 帧" << std::endl;
            }
        }
    }
    
    // 输出统计信息
    std::cout << "\n========== 统计信息 ==========" << std::endl;
    std::cout << "程序执行完成！共捕获 " << frame_count << " 帧" << std::endl;
    
    if (camera.hasFirstFrame()) {
        auto end_time = std::chrono::steady_clock::now();
        auto total_duration = std::chrono::duration_cast<std::chrono::milliseconds>(
            end_time - camera.getFirstFrameTime());
        
        std::cout << "总运行时长: " << total_duration.count() << " 毫秒" << std::endl;
        std::cout << "抓取成功的总次数: " << frame_count << " 次" << std::endl;
        
        if (frame_count > 1) {
            auto avg_interval = total_duration.count() / (frame_count - 1);
            std::cout << "抓取的平均间隔: " << avg_interval << " 毫秒" << std::endl;
        } else {
            std::cout << "抓取的平均间隔: 无法计算（次数不足）" << std::endl;
        }
    } else {
        std::cout << "未成功捕获任何帧" << std::endl;
    }
    std::cout << "==============================" << std::endl;
    
    return 0;
}

// ========== 多线程优化版本 ==========
#include <vector>
#include <queue>
#include <mutex>
#include <condition_variable>
#include <atomic>
#include <thread>

// 线程安全的帧数据队列（恢复到有效版本）
class FrameQueue {
private:
    std::queue<std::pair<void*, size_t>> frames;
    std::mutex queue_mutex;
    std::condition_variable not_empty;
    std::condition_variable not_full;
    size_t max_size;
    std::atomic<bool> shutdown{false};
    
public:
    FrameQueue(size_t max_frames = 10) : max_size(max_frames) {}
    
    // 生产者：添加帧数据
    bool push(void* data, size_t size) {
        std::unique_lock<std::mutex> lock(queue_mutex);
        
        // 等待队列有空间
        not_full.wait(lock, [this] { 
            return frames.size() < max_size || shutdown.load(); 
        });
        
        if (shutdown.load()) return false;
        
        frames.push({data, size});
        not_empty.notify_one();
        return true;
    }
    
    // 消费者：获取帧数据
    bool pop(std::pair<void*, size_t>& frame) {
        std::unique_lock<std::mutex> lock(queue_mutex);
        
        // 等待队列有数据
        not_empty.wait(lock, [this] { 
            return !frames.empty() || shutdown.load(); 
        });
        
        if (shutdown.load() && frames.empty()) return false;
        
        frame = frames.front();
        frames.pop();
        not_full.notify_one();
        return true;
    }
    
    // 关闭队列
    void shutdown_queue() {
        shutdown.store(true);
        not_empty.notify_all();
        not_full.notify_all();
    }
    
    // 获取队列大小
    size_t size() const {
        std::lock_guard<std::mutex> lock(const_cast<std::mutex&>(queue_mutex));
        return frames.size();
    }
    
    // 检查是否为空
    bool empty() const {
        std::lock_guard<std::mutex> lock(const_cast<std::mutex&>(queue_mutex));
        return frames.empty();
    }
};

// 多线程优化的V4L2摄像头类
class V4L2CameraMultiThread {
private:
    int video_fd;
    std::string video_device;
    int width, height;
    bool save_files;
    
    // 内存映射相关
    struct Buffer {
        void* start;
        size_t length;
        int index;
        bool in_use;
    };
    Buffer* buffers;
    unsigned int n_buffers;
    
    // 多线程相关
    FrameQueue frame_queue;
    std::thread capture_thread;
    std::thread process_thread;
    std::atomic<bool> running{false};
    std::atomic<int> captured_frames{0};
    
    // 计时相关
    std::chrono::steady_clock::time_point first_frame_time;
    std::chrono::steady_clock::time_point last_frame_time;
    bool first_frame_captured;
    
    // 统计信息
    std::atomic<long long> total_capture_time{0};
    std::atomic<long long> total_process_time{0};
    
public:
    V4L2CameraMultiThread(const std::string& video_dev = "/dev/video11", bool save = false) 
        : video_device(video_dev), width(480), height(480), save_files(save),
          buffers(nullptr), n_buffers(0), first_frame_captured(false) {}
    
    ~V4L2CameraMultiThread() {
        stop();
        cleanup();
    }
    
    bool openVideoDevice() {
        video_fd = open(video_device.c_str(), O_RDWR);
        if (video_fd == -1) {
            std::cerr << "无法打开视频设备 " << video_device << ": " << strerror(errno) << std::endl;
            return false;
        }
        std::cout << "成功打开视频设备: " << video_device << std::endl;
        return true;
    }
    
    bool setVideoFormat(int width, int height, __u32 pixelformat) {
        this->width = width;
        this->height = height;
        
        v4l2_format fmt;
        memset(&fmt, 0, sizeof(fmt));
        fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        fmt.fmt.pix_mp.width = width;
        fmt.fmt.pix_mp.height = height;
        fmt.fmt.pix_mp.pixelformat = pixelformat;
        fmt.fmt.pix_mp.field = V4L2_FIELD_NONE;
        fmt.fmt.pix_mp.num_planes = 2;
        
        if (ioctl(video_fd, VIDIOC_S_FMT, &fmt) < 0) {
            std::cerr << "设置多平面格式失败: " << strerror(errno) << std::endl;
            return false;
        }
        
        std::cout << "设置多平面视频格式成功: " << this->width << "x" << this->height 
                  << ", 像素格式: " << (char*)&pixelformat << std::endl;
        return true;
    }
    
    bool initMemoryMapping() {
        const int buffer_count = 4; // 恢复到之前有效的4个缓冲区
        
        v4l2_requestbuffers req;
        memset(&req, 0, sizeof(req));
        req.count = buffer_count;
        req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        req.memory = V4L2_MEMORY_MMAP;
        
        if (ioctl(video_fd, VIDIOC_REQBUFS, &req) < 0) {
            std::cerr << "请求多平面缓冲区失败: " << strerror(errno) << std::endl;
            return false;
        }
        
        if (req.count < 1) {
            std::cerr << "无法分配缓冲区" << std::endl;
            return false;
        }
        
        std::cout << "实际分配的缓冲区数量: " << req.count << std::endl;
        
        n_buffers = req.count;
        buffers = new Buffer[n_buffers];
        
        for (unsigned int i = 0; i < n_buffers; i++) {
            v4l2_plane planes[VIDEO_MAX_PLANES];
            v4l2_buffer buf;
            memset(&buf, 0, sizeof(buf));
            buf.index = i;
            buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
            buf.memory = V4L2_MEMORY_MMAP;
            buf.m.planes = planes;
            buf.length = 1;
            
            if (ioctl(video_fd, VIDIOC_QUERYBUF, &buf) < 0) {
                std::cerr << "查询多平面缓冲区 " << i << " 失败: " << strerror(errno) << std::endl;
                return false;
            }
            
            buffers[i].start = mmap(NULL, buf.m.planes[0].length, PROT_READ | PROT_WRITE, 
                                   MAP_SHARED, video_fd, buf.m.planes[0].m.mem_offset);
            if (buffers[i].start == MAP_FAILED) {
                std::cerr << "映射缓冲区 " << i << " 失败: " << strerror(errno) << std::endl;
                return false;
            }
            
            buffers[i].length = buf.m.planes[0].length;
            buffers[i].index = i;
            buffers[i].in_use = false;
            
            std::cout << "多平面缓冲区 " << i << " 映射成功，长度: " << buf.m.planes[0].length << " 字节" << std::endl;
        }
        
        std::cout << "内存映射初始化成功，缓冲区数量: " << n_buffers << std::endl;
        
        // 执行内存预分配优化
        preallocateMemory();
        
        return true;
    }
    
    bool startStreaming() {
        __u32 type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        
        if (ioctl(video_fd, VIDIOC_STREAMON, &type) < 0) {
            std::cerr << "开始多平面流失败: " << strerror(errno) << std::endl;
            return false;
        }
        
        std::cout << "多平面流启动成功，开始多线程捕获..." << std::endl;
        return true;
    }
    
    bool stopStreaming() {
        __u32 type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        
        if (ioctl(video_fd, VIDIOC_STREAMOFF, &type) < 0) {
            std::cerr << "停止多平面流失败: " << strerror(errno) << std::endl;
            return false;
        } else {
            std::cout << "多平面流已停止" << std::endl;
        }
        return true;
    }
    
    // 启动多线程处理
    bool start() {
        if (running.load()) return false;
        
        running.store(true);
        
        // 启动捕获线程
        capture_thread = std::thread(&V4L2CameraMultiThread::captureLoop, this);
        
        // 启动处理线程
        process_thread = std::thread(&V4L2CameraMultiThread::processLoop, this);
        
        // 设置CPU亲和性优化
        setThreadAffinity();
        
        std::cout << "多线程处理已启动（CPU亲和性优化）" << std::endl;
        return true;
    }
    
    // 停止多线程处理
    void stop() {
        if (!running.load()) return;
        
        running.store(false);
        frame_queue.shutdown_queue();
        
        if (capture_thread.joinable()) {
            capture_thread.join();
        }
        
        if (process_thread.joinable()) {
            process_thread.join();
        }
        
        std::cout << "多线程处理已停止" << std::endl;
    }
    
    // 获取统计信息
    int getCapturedFrames() const { return captured_frames.load(); }
    long long getTotalCaptureTime() const { return total_capture_time.load(); }
    long long getTotalProcessTime() const { return total_process_time.load(); }
    bool hasFirstFrame() const { return first_frame_captured; }
    std::chrono::steady_clock::time_point getFirstFrameTime() const { return first_frame_time; }
    
private:
    // 捕获线程主循环
    void captureLoop() {
        std::cout << "捕获线程启动" << std::endl;
        
        // 设置线程优先级
        struct sched_param param;
        param.sched_priority = 90; // 最高优先级
        pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);
        
        while (running.load()) {
            auto capture_start = std::chrono::steady_clock::now();
            
            if (captureFrame()) {
                captured_frames++;
                auto capture_end = std::chrono::steady_clock::now();
                auto capture_time = std::chrono::duration_cast<std::chrono::microseconds>(
                    capture_end - capture_start);
                total_capture_time += capture_time.count();
                
                                 // 恢复到之前有效的延时
                 usleep(50); // 50μs，之前有效的配置
            } else {
                usleep(1000); // 1ms，避免过度重试
            }
        }
        
        std::cout << "捕获线程退出" << std::endl;
    }
    
    // 处理线程主循环
    void processLoop() {
        std::cout << "处理线程启动" << std::endl;
        
        // 设置线程优先级
        struct sched_param param;
        param.sched_priority = 85; // 高优先级，仅次于捕获线程
        pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);
        
        // 批量处理优化
        std::vector<std::pair<void*, size_t>> batch_frames;
        batch_frames.reserve(10); // 预分配空间
        
        while (running.load()) {
            batch_frames.clear();
            
            // 批量获取帧数据，减少锁竞争
            for (int i = 0; i < 5 && running.load(); i++) {
                std::pair<void*, size_t> frame_data;
                if (frame_queue.pop(frame_data)) {
                    batch_frames.push_back(frame_data);
                } else {
                    break;
                }
            }
            
            if (!batch_frames.empty()) {
                auto process_start = std::chrono::steady_clock::now();
                
                // 批量处理帧数据
                for (const auto& frame_data : batch_frames) {
                    if (save_files) {
                        processFrame(frame_data.first, frame_data.second);
                    }
                }
                
                auto process_end = std::chrono::steady_clock::now();
                auto process_time = std::chrono::duration_cast<std::chrono::microseconds>(
                    process_end - process_start);
                total_process_time += process_time.count();
            } else {
                // 队列为空，短暂等待
                std::this_thread::sleep_for(std::chrono::microseconds(50)); // 减少等待时间
            }
        }
        
        std::cout << "处理线程退出" << std::endl;
    }
    
    // 捕获单帧（恢复到有效版本）
    bool captureFrame() {
        
        // 简单的循环缓冲区策略
        static int current_buffer = 0;
        int available_buffer = -1;
        
        // 查找可用缓冲区
        for (unsigned int i = 0; i < n_buffers; i++) {
            int check_index = (current_buffer + i) % n_buffers;
            if (!buffers[check_index].in_use) {
                available_buffer = check_index;
                break;
            }
        }
        
        if (available_buffer == -1) {
            // 所有缓冲区都在使用中，跳过这一帧
            return false;
        }
        
        v4l2_plane planes[VIDEO_MAX_PLANES];
        memset(planes, 0, sizeof(planes));
        
        v4l2_buffer buf;
        memset(&buf, 0, sizeof(buf));
        buf.index = available_buffer;
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.m.planes = planes;
        buf.length = 1;
        
        planes[0].m.mem_offset = 0;
        planes[0].length = buffers[available_buffer].length;
        
        if (ioctl(video_fd, VIDIOC_QBUF, &buf) < 0) {
            std::cerr << "入队缓冲区失败: " << strerror(errno) << std::endl;
            return false;
        }
        
        // 非阻塞poll，快速检查数据就绪
        struct pollfd pfd;
        pfd.fd = video_fd;
        pfd.events = POLLIN;
        pfd.revents = 0;
        
        int r = poll(&pfd, 1, 2); // 2ms超时，极速响应
        if (r <= 0) {
            // 超时或失败，出队缓冲区
            ioctl(video_fd, VIDIOC_DQBUF, &buf);
            return false;
        }
        
        // 出队缓冲区
        if (ioctl(video_fd, VIDIOC_DQBUF, &buf) < 0) {
            std::cerr << "出队缓冲区失败: " << strerror(errno) << std::endl;
            return false;
        }
        
        // 记录时间
        auto current_time = std::chrono::steady_clock::now();
        if (!first_frame_captured) {
            first_frame_time = current_time;
            first_frame_captured = true;
            std::cout << "第一帧捕获成功，开始计时..." << std::endl;
        }
        last_frame_time = current_time;
        
                 // 将帧数据加入队列
         if (frame_queue.push(buffers[available_buffer].start, planes[0].bytesused)) {
             // 更新缓冲区状态
             buffers[available_buffer].in_use = true;
             current_buffer = (current_buffer + 1) % n_buffers; // 更新当前缓冲区
             
             // 计算帧间隔
             if (captured_frames.load() > 0) {
                 auto frame_interval = std::chrono::duration_cast<std::chrono::microseconds>(
                     current_time - last_frame_time);
                 if (captured_frames.load() % 50 == 0) { // 每50帧输出一次
                     std::cout << "帧间隔: " << frame_interval.count() << " μs (" 
                               << frame_interval.count() / 1000.0 << " ms)" << std::endl;
                     
                     // 性能警告
                     if (frame_interval.count() > 6000) { // 超过6ms
                         std::cout << "⚠️  性能警告：帧间隔过高，可能需要优化" << std::endl;
                     } else if (frame_interval.count() < 5600) { // 低于5.6ms
                         std::cout << "🎯 达到目标性能！" << std::endl;
                     }
                 }
             }
             
             return true;
         }
        
        return false;
    }
    
    // 处理帧数据（恢复到有效版本）
    void processFrame(void* data, size_t size) {
        static int file_count = 0;
        
        // 生成文件名
        std::stringstream ss;
        ss << "frame_" << std::setfill('0') << std::setw(6) << file_count++ << ".raw";
        std::string filename = ss.str();
        
        // 保存文件
        FILE* outfile = fopen(filename.c_str(), "wb");
        if (outfile) {
            fwrite(data, 1, size, outfile);
            fclose(outfile);
            
            if (file_count % 100 == 0) {
                std::cout << "已保存 " << file_count << " 个文件" << std::endl;
            }
        }
        
        // 标记缓冲区可用
        for (unsigned int i = 0; i < n_buffers; i++) {
            if (buffers[i].start == data) {
                buffers[i].in_use = false;
                break;
            }
        }
    }
    
         // 设置CPU亲和性优化
     void setThreadAffinity() {
         // 捕获线程绑定到CPU 0（高性能核心）
         cpu_set_t cpuset;
         CPU_ZERO(&cpuset);
         CPU_SET(0, &cpuset);
         
         if (capture_thread.joinable()) {
             pthread_setaffinity_np(capture_thread.native_handle(), sizeof(cpu_set_t), &cpuset);
             std::cout << "捕获线程绑定到CPU 0" << std::endl;
         }
         
         // 处理线程绑定到CPU 1
         CPU_ZERO(&cpuset);
         CPU_SET(1, &cpuset);
         
         if (process_thread.joinable()) {
             pthread_setaffinity_np(process_thread.native_handle(), sizeof(cpu_set_t), &cpuset);
             std::cout << "处理线程绑定到CPU 1" << std::endl;
         }
     }
     
     // 内存预分配优化（简化版本）
     void preallocateMemory() {
         // 只保留最有效的内存优化
         for (unsigned int i = 0; i < n_buffers; i++) {
             if (buffers[i].start) {
                 // 锁定内存页，防止被换出
                 mlock(buffers[i].start, buffers[i].length);
                 
                 // 预读取内存，提高访问速度
                 volatile char* ptr = static_cast<volatile char*>(buffers[i].start);
                 for (size_t j = 0; j < buffers[i].length; j += 4096) {
                     volatile char temp = ptr[j];
                     (void)temp; // 避免编译器警告
                 }
             }
         }
         
         std::cout << "内存预分配和锁定完成" << std::endl;
     }
     
     void cleanup() {
         stop();
         
         if (buffers) {
             for (unsigned int i = 0; i < n_buffers; ++i) {
                 if (buffers[i].start) {
                     munlock(buffers[i].start, buffers[i].length);
                     munmap(buffers[i].start, buffers[i].length);
                 }
             }
             delete[] buffers;
             buffers = nullptr;
         }
         
         if (video_fd != -1) {
             close(video_fd);
             video_fd = -1;
         }
     }
};

// 多线程版本的主函数
int main_multi_thread(int argc, char* argv[]) {
    bool save_files = false;
    int opt;
    
    // 解析命令行参数
    while ((opt = getopt(argc, argv, "sh")) != -1) {
        switch (opt) {
            case 's':
                save_files = true;
                break;
            case 'h':
                std::cout << "多线程优化版本用法: " << argv[0] << " [选项]" << std::endl;
                std::cout << "  -s              保存捕获的图像文件到磁盘" << std::endl;
                std::cout << "  -h              显示此帮助信息" << std::endl;
                return 0;
            default:
                std::cerr << "未知选项，使用 -h 查看帮助" << std::endl;
                return 1;
        }
    }
    
    std::cout << "V4L2 摄像头多线程优化版本 (sc035gs MIPI 180fps)" << std::endl;
    std::cout << "===============================================" << std::endl;
    std::cout << "文件保存模式: " << (save_files ? "启用" : "禁用") << std::endl;
    std::cout << "按 Ctrl+C 停止程序" << std::endl;
    
    // 设置信号处理
    signal(SIGINT, signalHandler);
    signal(SIGTERM, signalHandler);
    
    V4L2CameraMultiThread camera("/dev/video11", save_files);
    g_camera = nullptr; // 多线程版本不使用全局变量
    
    // 初始化摄像头
    if (!camera.openVideoDevice()) {
        return -1;
    }
    
    if (!camera.setVideoFormat(480, 480, V4L2_PIX_FMT_NV12)) {
        return -1;
    }
    
    if (!camera.initMemoryMapping()) {
        return -1;
    }
    
    if (!camera.startStreaming()) {
        std::cerr << "开始流失败" << std::endl;
        return -1;
    }
    
    // 启动多线程处理
    if (!camera.start()) {
        std::cerr << "启动多线程处理失败" << std::endl;
        return -1;
    }
    
    // 主线程监控性能
    auto start_time = std::chrono::steady_clock::now();
    int last_frame_count = 0;
    
    while (true) {
        std::this_thread::sleep_for(std::chrono::seconds(1));
        
        int current_frames = camera.getCapturedFrames();
        int frames_this_second = current_frames - last_frame_count;
        last_frame_count = current_frames;
        
        auto current_time = std::chrono::steady_clock::now();
        auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
            current_time - start_time);
        
        std::cout << "运行时间: " << elapsed.count() << "ms, "
                  << "总帧数: " << current_frames << ", "
                  << "当前FPS: " << frames_this_second << std::endl;
        
        if (camera.hasFirstFrame()) {
            auto total_duration = std::chrono::duration_cast<std::chrono::milliseconds>(
                current_time - camera.getFirstFrameTime());
            if (current_frames > 1) {
                double avg_interval = total_duration.count() / (double)(current_frames - 1);
                std::cout << "平均间隔: " << std::fixed << std::setprecision(2) 
                          << avg_interval << " ms" << std::endl;
            }
        }
        
        // 检查是否达到目标性能
        if (frames_this_second >= 180) {
            std::cout << "🎉 达到180fps目标！" << std::endl;
        }
    }
    
    return 0;
}
